import cv2
import numpy as np
def rotation_matrix_from_vectorr(r):
    """
    旋转向量和矩阵互相转化
    Args:
        r:三维的旋转向量或旋转矩阵

    Returns:
        旋转矩阵或旋转向量
    """
    r=np.float32(r)
    roation=cv2.Rodrigues(r)[0]
    return roation

def create_transformation_matrix(pose):
    """
    创建齐次矩阵
    Args:
        pose:六维向量，前三维XYZ，后三维旋转向量

    Returns:
        齐次矩阵
    """
    translation = np.eye(4)
    translation[:3, 3] = pose[:3]
    translation[:3,:3]=rotation_matrix_from_vectorr(pose[3:])
    return translation

def pos_inv(pose):
    m1=create_transformation_matrix(pose)
    inv=np.linalg.inv(m1)
    m_rot = inv[:3, :3]
    vec_m = rotation_matrix_from_vectorr(m_rot)
    v = [inv[0, 3], inv[1, 3], inv[2, 3], vec_m[0][0], vec_m[1][0], vec_m[2][0]]
    return v

def trans_pose(pose1,pose2):
    """
    两个姿态相乘，pose输入为6维向量，前三维为xyz，后三维为旋转向量（轴角）
    Args:
        pose1:
        pose2:

    Returns:
    转化后的向量
    """
    m1=create_transformation_matrix(pose1)
    m2=create_transformation_matrix(pose2)
    m=np.dot(m1,m2)
    m_rot=m[:3,:3]
    vec_m=rotation_matrix_from_vectorr(m_rot)
    v=[m[0,3],m[1,3],m[2,3],vec_m[0][0],vec_m[1][0],vec_m[2][0]]
    return v

# pose=create_transformation_matrix(np.array([1,2,3,0.0,0.0,0.0]))
# print(trans_pose(np.array([1,2,3,0.0,0.0,0.0]),np.array([1,0,0,0.0,0.0,0.0])))
